package algo;

import java.io.IOException;
import java.math.BigDecimal;
import java.math.RoundingMode;
import java.nio.file.Files;
import java.nio.file.Paths;
import java.util.*;
import java.util.stream.Collectors;

/**
 * 暂时没有加滤波，结果相同。
 */
public class DistanceAlgo {

        private double filteredA_mid_value = -53;//TODO  16可以
    private double filteredB_mid_value = -52;
//    private double filteredA_mid_value = -57;//TODO  13 数据可以
//    private double filteredB_mid_value = -56;
//    private double filteredA_mid_value = -53;//TODO
//    private double filteredB_mid_value = -52;

//    private double filteredA_mid_value = -57;//TODO  13 数据可以...16不可以
//    private double filteredB_mid_value = -58;

    public static boolean isShowLog = false;

    private DistanceAlgo() {
    }

    private static class DistanceAlgoHolder {
        static DistanceAlgo ins = new DistanceAlgo();
    }

    public static DistanceAlgo getInstance() {
        return DistanceAlgoHolder.ins;
    }


    public static void main(String[] args) throws IOException {
        List<Double> markA_origin = new ArrayList<>();
        List<Double> markB_origin = new ArrayList<>();
        List<Double> step_count_origin_data = new ArrayList<>();

        // 读取 markA 文件
//        List<String> markA_lines = Files.readAllLines(Paths.get("./src/algo/OriData/2024_07_11/check_kangfu_filter_69D1CC63B9F3_2024-07-11_10_28_19.txt"));
//        List<String> markA_lines = Files.readAllLines(Paths.get("./src/algo/newdata/temp_rssi_start_2024-08-01_09_06_22.txt"));
//        List<String> markA_lines = Files.readAllLines(Paths.get("./src/algo/newdata/check_kangfu_filter_69D1CC63B9F3_2024-08-01_09_06_22.txt"));
        List<String> markA_lines = Files.readAllLines(Paths.get("./src/algo/newdata2/temp_rssi_start_2024-08-01_11_22_16.txt"));//452.6572847682119 :max:::452.6572847682119
//        List<String> markA_lines = Files.readAllLines(Paths.get("./src/algo/newdata2_2/check_kangfu_filter_69D1CC63B9F3_2024-08-01_11_22_16.txt"));

        for (String line : markA_lines) {
            String[] parts = line.trim().split(",");
            markA_origin.add(Double.parseDouble(parts[1])); // markA 滤波
        }

        // 读取 markB 文件
//        List<String> markB_lines = Files.readAllLines(Paths.get("./src/algo/OriData/2024_07_11/check_kangfu_filter_480F1EB685D8_2024-07-11_10_28_19.txt"));
//        List<String> markB_lines = Files.readAllLines(Paths.get("./src/algo/temp/temp_rssi_end_2024-07-31_19_38_57.txt"));
//        List<String> markB_lines = Files.readAllLines(Paths.get("./src/algo/newdata/temp_rssi_end_2024-08-01_09_06_22.txt"));
//        List<String> markB_lines = Files.readAllLines(Paths.get("./src/algo/newdata/check_kangfu_filter_480F1EB685D8_2024-08-01_09_06_22.txt"));
        List<String> markB_lines = Files.readAllLines(Paths.get("./src/algo/newdata2/temp_rssi_end_2024-08-01_11_22_16.txt"));
//        List<String> markB_lines = Files.readAllLines(Paths.get("./src/algo/newdata2_2/check_kangfu_filter_480F1EB685D8_2024-08-01_11_22_16.txt"));
        for (String line : markB_lines) {
            String[] parts = line.trim().split(",");
            markB_origin.add(Double.parseDouble(parts[1])); // markB 滤波
        }

        // 读取步数文件
//        List<String> step_count_lines = Files.readAllLines(Paths.get("./src/algo/OriData/2024_07_11/check_kangfu_filter_walk_speed_2024-07-11_10_28_19.txt"));
//        List<String> step_count_lines = Files.readAllLines(Paths.get("./src/algo/temp/temp_step_distance_count_2024-07-31_19_38_57.txt"));
//        List<String> step_count_lines = Files.readAllLines(Paths.get("./src/algo/newdata/temp_step_distance_count_2024-08-01_09_06_22.txt"));
//        List<String> step_count_lines = Files.readAllLines(Paths.get("./src/algo/newdata/check_kangfu_filter_walk_speed_2024-08-01_09_06_22.txt"));
        List<String> step_count_lines = Files.readAllLines(Paths.get("./src/algo/newdata2/temp_step_distance_count_2024-08-01_11_22_16.txt"));
//        List<String> step_count_lines = Files.readAllLines(Paths.get("./src/algo/newdata2_2/check_kangfu_filter_walk_speed_2024-08-01_11_22_17.txt"));
        for (String line : step_count_lines) {
            String[] parts = line.trim().split(",");
            step_count_origin_data.add(Double.parseDouble(parts[4])); // 步数
        }

        // 调用mathDistance函数
        Result result = getInstance().mathDistance(markA_origin, markB_origin, step_count_origin_data);
        System.out.println("DistanceCalculator:main:----over:" + result.toString());
    }

    private volatile int twoMarkPointDistance = 30;//30米 默认
    private int markLenInit = 3800;//	Java 中的 double 类型占用 8 个字节 = 3800 * 8 = 30400 \text{ 字节}（约 30.4 KB）

    public void setTwoMarkPointDistance(int twoMarkPointDistance) {
        this.twoMarkPointDistance = twoMarkPointDistance;
//        System.out.println("DistanceAlgoR:setTwoMarkPointDistance: " + twoMarkPointDistance);
    }

    private double[] markA_origin = new double[markLenInit];
    private double[] markB_origin = new double[markLenInit];
    private double[] step_count_origin_data = new double[markLenInit / 10];
    //加滤波
    private volatile int lastFilterDataSize = 0;
    public double[] filtered_markA = new double[markLenInit];
    public double[] filtered_markB = new double[markLenInit];
    private KalmanFilter kalmanFilterA = new KalmanFilter();
    private KalmanFilter kalmanFilterB = new KalmanFilter();
    private StreamingMean aveA;
    private StreamingMean aveB;
    public void startCleanOriginData() {
        if (aveA!=null){aveA = null;}
        if (aveB!=null){aveB = null;}
        aveA = new StreamingMean();
        aveB = new StreamingMean();

        if (kalmanFilterA!=null){
            kalmanFilterA = null;
        }
        if (kalmanFilterB!=null){
            kalmanFilterB = null;
        }
        kalmanFilterA = new KalmanFilter();
        kalmanFilterB = new KalmanFilter();
        for (int i = 0; markA_origin != null && i < markA_origin.length; i++) {
            markA_origin[i] = 0;
            markB_origin[i] = 0;
        }
        for (int i = 0; step_count_origin_data != null && i < step_count_origin_data.length; i++) {
            step_count_origin_data[i] = 0;
        }
        lastFilterDataSize = 0;
        for (int i = 0; i < filtered_markA.length; i++) {
            filtered_markA[i] = 0;
            filtered_markB[i] = 0;
        }
        isStop = false;


        peekA_index_cache.clear();
        last_peekA_point = 0;
        last_right_indexA = 0;
        peekB_index_cache.clear();
        last_peekB_point = 0;
        last_right_indexB = 0;

        // TODO: 8/8/24 等待优化
        for (int i = 0; i < distance_origin.length; i++) {
            distance_origin[i] = 0;
            distance_origin_fix[i] = 0;
        }
        distance_origin_for_second.clear();
        distance_origin_for_second.add(0.0);

        distance_max = 0;
        distance_max_index = 0;

        is_half_circle_long = 1;  // 是否满足半圈长度
        speed_for_second.clear();
        last_speed = 0;


        last_rssi_average_A = 0;
        diff_2_point_rssiA_all.clear();
        RSSI_A_Ave_Result.clear();

        last_rssi_average_B = 0;
        diff_point_rssiB_all.clear();
        RSSI_B_Ave_Result.clear();

        big_A_mid_index.clear();
        lastAmidIndex = 0;
        big_B_mid_index.clear();
        lastBmidIndex = 0;

        guaidian_index.clear();
        guaidian_index.add(0);  // 所有拐点
        evert_mark_point_steps_count.clear();  // 步数


        mid_lind_a_index_cache.clear();
        mid_lind_a_index_cache.add(0);
        mid_lind_a_origin_set.clear();
        mid_lind_b_index_cache.clear();
        mid_lind_b_index_cache.add(0);
        mid_lind_b_origin_set.clear();

        mid_lind_a_last_Index = 0;
        mid_lind_b_last_Index = 0;
        mathClean();
    }

    private volatile boolean isStop = true;

    public void onStop() {
        startCleanOriginData();
        isStop = true;
    }


    private double[] distance_origin = new double[markLenInit + 1];
    private double[] distance_origin_fix = new double[markLenInit + 1];
    //TODO 等待优化成数组，
    private List<Double> distance_origin_for_second = new ArrayList<>();
    private volatile double distance_max = 0;//结果展示用
    private volatile int distance_max_index = 0;

    private int is_half_circle_long = 1;  // 是否满足半圈长度
    private List<Double> speed_for_second = new ArrayList<>();//TODO  输出！！！！！！！！！！！！
    public volatile double last_speed = 0;

    private double last_rssi_average_A = 0;
    private List<Double> diff_2_point_rssiA_all = new ArrayList<>();
    //    private List<Double> diff_2_point_rssiA_all = new CopyOnWriteArrayList<>();//500
    //500 step
    private List<Double> RSSI_A_Ave_Result = new ArrayList<>();
    //    private List<Double> RSSI_A_Ave_Result = new CopyOnWriteArrayList<>();//500
    private double last_rssi_average_B = 0;
    private List<Double> diff_point_rssiB_all = new ArrayList<>();
    //    private List<Double> diff_point_rssiB_all = new CopyOnWriteArrayList<>();//500
    private List<Double> RSSI_B_Ave_Result = new ArrayList<>();
    //    private List<Double> RSSI_B_Ave_Result = new CopyOnWriteArrayList<>();//500
    private List<Integer> guaidian_index = new ArrayList<>();
    private List<Integer> evert_mark_point_steps_count = new ArrayList<>();  // 步数
    private int isMoving = 0;
    private double offset_rice = 5;  // m
    private double one_step_length = 0.6;  // 初始半圈步长，m



    private List<Integer> big_A_mid_index = new ArrayList<>();
    private volatile int lastAmidIndex = 0;
    private List<Integer> big_B_mid_index = new ArrayList<>();
    private volatile int lastBmidIndex = 0;

    private HashSet<Integer> mid_lind_a_index_cache = new HashSet<>();
    private HashSet<Integer> mid_lind_a_origin_set = new HashSet<>();
    private HashSet<Integer> mid_lind_b_index_cache = new HashSet<>();
    private HashSet<Integer> mid_lind_b_origin_set = new HashSet<>();
    private int mid_lind_a_last_Index = 0;
    private int mid_lind_b_last_Index = 0;

    public HashSet<Integer> peekA_index_cache = new HashSet();
    private volatile int last_peekA_point = 0;
    private volatile int last_right_indexA = 0;
    public HashSet<Integer> peekB_index_cache = new HashSet();
    private volatile int last_peekB_point = 0;
    private volatile int last_right_indexB = 0;

    public void mathClean() {
//        peekA_index_cache.clear();
//        last_peekA_point = 0;
//        last_right_indexA = 0;
//        peekB_index_cache.clear();
//        last_peekB_point = 0;
//        last_right_indexB = 0;

//        // TODO: 8/8/24 等待优化
//        for (int i = 0; i < distance_origin.length; i++) {
//            distance_origin[i] = 0;
//            distance_origin_fix[i] = 0;
//        }
//        distance_origin_for_second.clear();
//        distance_origin_for_second.add(0.0);

//        distance_max = 0;
//        distance_max_index = 0;

        is_half_circle_long = 1;  // 是否满足半圈长度
//        speed_for_second.clear();

//        last_speed = 0;

//        last_rssi_average_A = 0;
//        diff_2_point_rssiA_all.clear();
//        RSSI_A_Ave_Result.clear();

//        last_rssi_average_B = 0;
//        diff_point_rssiB_all.clear();
//        RSSI_B_Ave_Result.clear();

//        guaidian_index.clear();
//        guaidian_index.add(0);  // 所有拐点
//        evert_mark_point_steps_count.clear();  // 步数
        isMoving = 0;
        offset_rice = 5;  // m
        one_step_length = 0.6;  // 初始半圈步长，m

//        big_A_mid_index.clear();
//        lastAmidIndex = 0;
//        big_B_mid_index.clear();
//        lastBmidIndex = 0;

//        mid_lind_a_index_cache.clear();
//        mid_lind_a_index_cache.add(0);
//        mid_lind_a_origin_set.clear();
//        mid_lind_b_index_cache.clear();
//        mid_lind_b_index_cache.add(0);
//        mid_lind_b_origin_set.clear();
    }


    private volatile int markANextReceiveIndex = 0;

    //只会 1 个线程发送。
    public void receiveDataStartMark(double markA) {
        if (isStop) {
            return;
        }
        // 数组已满，进行扩容
        if (markA_origin.length <= markANextReceiveIndex + 1) {
            markA_origin = Arrays.copyOf(markA_origin, (int) (markA_origin.length * 1.2));
        }
        markA_origin[markANextReceiveIndex] = markA;
        markANextReceiveIndex++;
    }

    private volatile int markBNextReceiveIndex = 0;

    public void receiveDataEndMark(double markB) {
        if (isStop) {
            return;
        }
        // 数组已满，进行扩容
        if (markB_origin.length <= markBNextReceiveIndex + 1) {
            markB_origin = Arrays.copyOf(markB_origin, (int) (markB_origin.length * 1.2));
        }
        markB_origin[markBNextReceiveIndex] = markB;
        markBNextReceiveIndex++;
    }

    private volatile int stepNextReceiveIndex = 0;

    public void receiveStepData(double step_count_data) {
        if (isStop) {
            return;
        }
        // 数组已满，进行扩容
        if (step_count_origin_data.length <= stepNextReceiveIndex + 1) {
            step_count_origin_data = Arrays.copyOf(step_count_origin_data, (int) (step_count_origin_data.length * 1.2));
        }
        step_count_origin_data[stepNextReceiveIndex] = step_count_data;
        stepNextReceiveIndex++;
    }

    private volatile boolean isDistanceMathing = false;

    public boolean isDistanceMathing() {
        return isDistanceMathing;
    }

    public Result mathDistance() {
        isDistanceMathing = true;
        Result result = mathDistanceNew(this.markA_origin, this.markB_origin, this.step_count_origin_data);
        isDistanceMathing = false;
        return result;
    }

    public Result mathDistance(List<Double> markA_origin, List<Double> markB_origin, List<Double> step_count_origin_data) {
        isStop = false;
        markANextReceiveIndex = markA_origin.size();
        markBNextReceiveIndex = markB_origin.size();
        stepNextReceiveIndex = step_count_origin_data.size();
        return mathDistanceNew(markA_origin.stream().mapToDouble(Double::doubleValue).toArray()
                , markB_origin.stream().mapToDouble(Double::doubleValue).toArray()
                , step_count_origin_data.stream().mapToDouble(Double::doubleValue).toArray());
    }


    public Result mathDistanceNew(double[] markA_origin, double[] markB_origin, double[] step_count_origin_data) {
        if (isStop) {
            System.out.println("DistanceAlgo:mathDistance:is already stop math");
            return null;
        }
        if (markA_origin.length < 1 || markB_origin.length < 1 || step_count_origin_data.length < 1) {
            System.out.println("DistanceAlgo:mathDistance: A size:" + markA_origin.length + ",B size:" + markB_origin.length + ",step size:" + step_count_origin_data.length);
            return null;
        }
        if (distance_origin.length <= markA_origin.length) {
            // 数组已满，进行扩容
            distance_origin = Arrays.copyOf(distance_origin, (int) (distance_origin.length * 1.2));
            distance_origin_fix = Arrays.copyOf(distance_origin_fix, (int) (distance_origin_fix.length * 1.2));
        }

        // TODO: 8/6/24 better----不能每次都 clean

        //TODO A B 取最小值，                  不考虑，速率不等。。。。或者最后再算速率不等的情况。
        int minPointSize = Math.min(markANextReceiveIndex, markBNextReceiveIndex);
//        if (minPointSize > stepNextReceiveIndex * 10){ //速率不统一。。。。加上，偏差变大。
//            minPointSize = stepNextReceiveIndex * 10;
//        }
        if (markANextReceiveIndex < 1) {
            System.out.println("DistanceAlgo:mathDistanceNew: no data");
            return null;
        }

        //这个是正确的。。。。不用这个。
//        for (int i = lastFilterDataSize; i < minPointSize; i++) {
//            //添加滤波
//            if (i >= 10) { // TODO 滑动平均滤波 正确的地方，为了统一，按照错误的进行。
//                double sum_A = 0;
//                for (int j = i - 10; j < i; j++) {
//                    sum_A += markA_origin[j];
//                }
//                filtered_markA[i] = sum_A / 10;
//                double sum_B = 0;
//                for (int j = i - 10; j < i; j++) {
//                    sum_B += markB_origin[j];
//                }
//                filtered_markB[i] = sum_B / 10;
//            } else {
//                filtered_markA[i] = markA_origin[i];
//                filtered_markB[i] = markB_origin[i];
//            }
//        }

        // 数组已满，进行扩容
        if (filtered_markA.length <= lastFilterDataSize || minPointSize > filtered_markA.length) {
            int max = Math.max(minPointSize + 10, (int) (filtered_markA.length * 1.2));
            System.out.println("DistanceAlgo:mathDistanceNew:扩容前：" + filtered_markA.length + "扩容后：" + max);
            filtered_markA = Arrays.copyOf(filtered_markA, max);
            filtered_markB = Arrays.copyOf(filtered_markB, max);
        }

        //        添加滤波 新的滤波
        if (lastFilterDataSize == 0 && minPointSize > 0){
//            System.err.println("DistanceAlgo:mathDistanceNew:----------kf init");
            kalmanFilterA.estimatedValue = markA_origin[0];
            kalmanFilterB.estimatedValue = markB_origin[0];
        }
        for (int i = lastFilterDataSize; i < minPointSize; i++) {
                filtered_markA[i] = kalmanFilterA.update(markA_origin[i]);
//                 System.out.println("DistanceAlgo:mathDistanceNew:"+i+"origin:"+markA_origin[i]+" res:"+filtered_markA[i]);
                filtered_markB[i] = kalmanFilterB.update(markB_origin[i]);
                aveA.update(markA_origin[i]);
                aveB.update(markB_origin[i]);
        }

////        添加滤波
//        for (int i = lastFilterDataSize; i < minPointSize; i++) {
//            if (i >= 9) { // 滑动平均滤波
//                double sum_A = 0;
//                for (int j = i - 9; j <= i; j++) {
//                    sum_A += markA_origin[j];
//                }
//                filtered_markA[i] = sum_A / 10;
//                double sum_B = 0;
//                for (int j = i - 9; j <= i; j++) {
//                    sum_B += markB_origin[j];
//                }
//                filtered_markB[i] = sum_B / 10;
//            } else {
//                filtered_markA[i] = markA_origin[i];
//                filtered_markB[i] = markB_origin[i];
//            }
//        }


        mathClean();
        int aPointIndex = 1;
        if (lastFilterDataSize != 0 && lastFilterDataSize > 1) {
            aPointIndex = lastFilterDataSize;
        }
//        System.out.println("DistanceAlgo:mathDistanceNew:-------aPointIndexStart:"+aPointIndex);
//        for (int aPointIndex = 1; aPointIndex < minPointSize; aPointIndex++) {
        for (; aPointIndex < minPointSize; aPointIndex++) {
            onMathRssiStepCount(step_count_origin_data, aPointIndex);
        }


//        // 获取波峰的索引列表
//        List<Integer> peakIndices = PeakDetection.detectPeaks(filtered_markA);
//        // 输出波峰的索引
//        System.out.println("Detected peak indices:AAA " + peakIndices);

        lastFilterDataSize = minPointSize;
        return new Result(distance_origin_fix, distance_origin_fix[markANextReceiveIndex - 1], markANextReceiveIndex - 1, distance_max, distance_max_index, step_count_origin_data[stepNextReceiveIndex - 1], one_step_length, speed_for_second);
    }


    //5 point ave
    private static double calculateAverage(double[] filtered_markA, int aveLastIndex) {
        int start = Math.max(aveLastIndex - 4, 0);
        double sum = 0.0;
        int count = 0;

        for (int i = start; i <= aveLastIndex; i++) {
            sum += filtered_markA[i];
            count++;
        }

        return count == 0 ? 0.0 : sum / count;
    }

    private void onMathRssiStepCount(double[] step_count_origin_data, int aPointIndex) {
        // 代码的主要逻辑部分

        if (aPointIndex < 500) {
//        if (aPointIndex < 4000) {
            //5 个数
            double rssi_A_average = calculateAverage(filtered_markA, aPointIndex);


            double rssi_B_average = calculateAverage(filtered_markB, aPointIndex);

            RSSI_A_Ave_Result.add(rssi_A_average);
            RSSI_B_Ave_Result.add(rssi_B_average);
            double distance_math_temp = 0;

            //判断运动状态
            int time_second_index = (int) Math.floor((aPointIndex - 1) / 10.0) - 1;
            if (time_second_index < 0) {
                evert_mark_point_steps_count.add((int) step_count_origin_data[0]);
//            } else if (time_second_index < step_count_origin_data.size()) {
            } else if (time_second_index < stepNextReceiveIndex) {
                evert_mark_point_steps_count.add((int) step_count_origin_data[time_second_index]);
            } else {
                evert_mark_point_steps_count.add((int) step_count_origin_data[stepNextReceiveIndex - 1]);
            }

            //isMoving
            int last_step_8sum = 0;
            if (evert_mark_point_steps_count.size() >= 100) {
                for (int i = -100; i < -20; i += 10) {
                    last_step_8sum += evert_mark_point_steps_count.get(Math.max((evert_mark_point_steps_count.size() + i + 10), 0)) - evert_mark_point_steps_count.get(Math.max(evert_mark_point_steps_count.size() + i, 0));
                }
                last_step_8sum /= 8;
                if (last_step_8sum >= 0.5) {
                    isMoving = 1;
                }
            }

            //计算拐点和距离
            double MAX_RSSI_Last40Point = RSSI_A_Ave_Result.subList(Math.max(RSSI_A_Ave_Result.size() - 40, 0), RSSI_A_Ave_Result.size()).stream().mapToDouble(val -> val).max().orElse(-90.0);
            double diff_last_now = rssi_A_average - last_rssi_average_A;
            diff_2_point_rssiA_all.add(diff_last_now);
            long diff_size_condition = diff_2_point_rssiA_all.subList(Math.max(diff_2_point_rssiA_all.size() - 15, 0), diff_2_point_rssiA_all.size()).stream().filter(val -> val < 0).count();

            if (rssi_A_average < -40 && rssi_A_average > -45 && aPointIndex - 1 - guaidian_index.get(guaidian_index.size() - 1) >= 100 && isMoving == 1 && diff_size_condition > 5 && MAX_RSSI_Last40Point > -41) {
                guaidian_index.add(aPointIndex - 1);
                if (guaidian_index.size() == 2) {
                    distance_math_temp = twoMarkPointDistance + offset_rice;
                    one_step_length = distance_math_temp / evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1);
                } else {
                    distance_math_temp = distance_origin[(guaidian_index.get(guaidian_index.size() - 2))] + twoMarkPointDistance;
                    one_step_length = twoMarkPointDistance / Math.abs(evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1) - (int) step_count_origin_data[Math.max(0, (int) Math.floor(guaidian_index.get(guaidian_index.size() - 2) / 10.0) - 1)]);
                }
            } else {
                if ((int) Math.floor(guaidian_index.get(guaidian_index.size() - 1) / 10.0) <= stepNextReceiveIndex - 1) {
                    distance_math_temp = distance_origin[(guaidian_index.get(guaidian_index.size() - 1))] + one_step_length * Math.abs(evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1) - (int) step_count_origin_data[(int) Math.floor(guaidian_index.get(guaidian_index.size() - 1) / 10.0)]);
                } else {
                    distance_math_temp = distance_origin[guaidian_index.get(guaidian_index.size() - 1)] + one_step_length * Math.abs(evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1) - (int) step_count_origin_data[stepNextReceiveIndex - 1]);
                }
            }
            last_rssi_average_A = rssi_A_average;

            //B mark
            MAX_RSSI_Last40Point = RSSI_B_Ave_Result.subList(Math.max(RSSI_B_Ave_Result.size() - 40, 0), RSSI_B_Ave_Result.size()).stream().mapToDouble(val -> val).max().orElse(-90.0);
            diff_last_now = rssi_B_average - last_rssi_average_B;
            diff_point_rssiB_all.add(diff_last_now);
            diff_size_condition = diff_point_rssiB_all.subList(Math.max(diff_point_rssiB_all.size() - 15, 0), diff_point_rssiB_all.size()).stream().filter(val -> val < 0).count();

            if (rssi_B_average < -40 && rssi_B_average > -45 && aPointIndex - 1 - guaidian_index.get(guaidian_index.size() - 1) >= 200 && isMoving == 1 && diff_size_condition > 5 && MAX_RSSI_Last40Point > -41) {
                guaidian_index.add(aPointIndex - 1);
                if (guaidian_index.size() == 2) {
                    distance_math_temp = twoMarkPointDistance + offset_rice;
                    one_step_length = (twoMarkPointDistance + offset_rice) / evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1);
                } else {
                    distance_math_temp = distance_origin[guaidian_index.get(guaidian_index.size() - 2)] + twoMarkPointDistance;
                    one_step_length = twoMarkPointDistance / Math.abs(evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1) - (int) step_count_origin_data[Math.max(0, (int) Math.floor(guaidian_index.get(guaidian_index.size() - 2) / 10.0) - 1)]);
                }
            } else {
                if ((int) Math.floor(guaidian_index.get(guaidian_index.size() - 1) / 10.0) <= stepNextReceiveIndex - 1) {
                    distance_math_temp = distance_origin[guaidian_index.get(guaidian_index.size() - 1)] + one_step_length * Math.abs(evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1) - (int) step_count_origin_data[(int) Math.floor(guaidian_index.get(guaidian_index.size() - 1) / 10.0)]);
                } else {
                    distance_math_temp = distance_origin[guaidian_index.get(guaidian_index.size() - 1)] + one_step_length * Math.abs(evert_mark_point_steps_count.get(evert_mark_point_steps_count.size() - 1) - (int) step_count_origin_data[stepNextReceiveIndex - 1]);
                }
            }
            last_rssi_average_B = rssi_B_average;


            distance_origin[aPointIndex] = distance_math_temp;
            distance_origin_fix[aPointIndex] = distance_origin[aPointIndex];

            double distance_last = distance_origin_fix[aPointIndex - 1];
            double distance_cur = distance_origin_fix[aPointIndex];
            if (distance_last > distance_cur) {
                distance_origin_fix[aPointIndex] = distance_last;
            }

//       //------------------👇不能注释
            if (aPointIndex % 10 == 0) {
                distance_origin_for_second.add(distance_origin_fix[aPointIndex]);
                if (distance_origin_for_second.get(distance_origin_for_second.size() - 1) < 5) {
                    speed_for_second.add(0.0);
                    if (isShowLog) {
                        System.out.println("speed-----11   >" + 0 + "   apoint:" + aPointIndex);
                    }
                } else {
                    int last_5_second = (int) (aPointIndex / 10.0) - 5;
                    if (last_5_second < 0) {
                        last_5_second = 0;
                    }
                    double dis_5_second = distance_origin_for_second.get(distance_origin_for_second.size() - 1) - distance_origin_for_second.get(last_5_second);
                    double cur_speed = dis_5_second / 5.0;
                    if (cur_speed - last_speed > 0.5) {
                        cur_speed = last_speed + 0.1;
                    }
                    if (cur_speed < 0) {
                        cur_speed = 0;
                    }
                    if (isShowLog) {
                        System.out.println("speed-----12   >" + cur_speed + "   apoint:" + aPointIndex);
                        System.out.print("");
                    }
                    last_speed = cur_speed;
                    speed_for_second.add(cur_speed);
                }
            }
        } else {
            // 大于 500 个点时。。。。
            // 计算波峰
//            double filteredA_max_value = Arrays.stream(filtered_markA,200 ,aPointIndex).max().orElse(-90.0);
//            double filteredA_min_value = Arrays.stream(filtered_markA,200,aPointIndex).min().orElse(-80.0);
//            double filteredA_mid_value = (filteredA_max_value + filteredA_min_value) / 2;
            // TODO: 8/4/24 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!  不需要算，取经验值 ok 省了 20ms
            int lastBigAmidSize = big_A_mid_index.size();
            for (int i = lastAmidIndex; i < aPointIndex + 1; i++) {
//                if (filtered_markA[i] > filteredA_mid_value) {
                if (filtered_markA[i] > aveA.getMean()) {
                    big_A_mid_index.add(i);
                }
            }

            int[] a = new int[0];
            int[] b = new int[0];
            if (big_A_mid_index.size() > 5 && lastBigAmidSize > 5) {
                if (big_A_mid_index.size() > lastBigAmidSize) {
                    a = big_A_mid_index.subList(lastBigAmidSize - 5, big_A_mid_index.size() - 1).stream().mapToInt(Integer::intValue).toArray();
                }
                if (big_A_mid_index.size() >= lastBigAmidSize + 1) {
                    b = big_A_mid_index.subList(lastBigAmidSize + 1 - 5, big_A_mid_index.size()).stream().mapToInt(Integer::intValue).toArray();
                }
            } else {
                if (big_A_mid_index.size() > lastBigAmidSize) {
                    a = big_A_mid_index.subList(lastBigAmidSize, big_A_mid_index.size() - 1).stream().mapToInt(Integer::intValue).toArray();
                }
                if (big_A_mid_index.size() >= lastBigAmidSize + 1) {
                    b = big_A_mid_index.subList(lastBigAmidSize + 1, big_A_mid_index.size()).stream().mapToInt(Integer::intValue).toArray();
                }
            }

            int[] cha = new int[a.length];
            for (int i = 0; i < a.length; i++) {
                cha[i] = b[i] - a[i];
            }
            int tempRightIndex = -1;
            List<Integer> interval_mid_line_index = new ArrayList<>();
            for (int i = 0; i < cha.length; i++) {
//                if (cha[i] > 150) {
                if (cha[i] > 80) {
                    int leftIndex = a[i];
                    int rightIndex = b[i];
                    interval_mid_line_index.add(leftIndex);
                    interval_mid_line_index.add(rightIndex);
                    mid_lind_a_origin_set.add(leftIndex);
                    mid_lind_a_origin_set.add(rightIndex);
                    mid_lind_a_index_cache.add(leftIndex);
                    mid_lind_a_index_cache.add(rightIndex);
                    if (rightIndex > mid_lind_a_last_Index) {
                        mid_lind_a_last_Index = rightIndex;
                    }
                    tempRightIndex = rightIndex;
                }
            }

            if (mid_lind_a_index_cache.size() > 0) {
                interval_mid_line_index.addAll(mid_lind_a_index_cache);
            }

            if (isShowLog) {
                interval_mid_line_index = interval_mid_line_index.stream().distinct().sorted().collect(Collectors.toList());
                System.out.println("DistanceAlgo:onMathRssiStepCount:interval_mid_line_index: a 交点" + interval_mid_line_index + "------apoint" + aPointIndex);
            }

            interval_mid_line_index.add(aPointIndex - 1);//TODO every p is new
            interval_mid_line_index = interval_mid_line_index.stream().distinct().sorted().collect(Collectors.toList());

            //TODO a 交点 找完。
            lastAmidIndex = aPointIndex;

            //TODO A 找到 波峰，索引。最大值
            List<Integer> peak_index_A = new ArrayList<>();
            if (last_peekA_point > 0 && peekA_index_cache.size() > 0) {
                peak_index_A.addAll(peekA_index_cache);
            }
//            double lastPeak_A_Rssi = filtered_markA[last_peekA_point];

            for (int i = 1; i < interval_mid_line_index.size(); i++) {
                int left = interval_mid_line_index.get(i - 1);
                if (left < last_right_indexA) {
                    continue;
                }
                int right = interval_mid_line_index.get(i);
                //找到最大值，索引。
                int peek_index = left;
                double maxValue = filtered_markA[left];
                for (int j = left; j <= right; j++) {
                    if (filtered_markA[j] > maxValue) {
                        maxValue = filtered_markA[j];
                        peek_index = j;
                    }
                }
                if (maxValue > filtered_markA[last_peekA_point] && (peek_index - last_peekA_point < 100
                        || (mid_lind_a_last_Index > -1 && peek_index > mid_lind_a_last_Index && last_peekA_point > mid_lind_a_last_Index))) {
                    peekA_index_cache.remove(last_peekA_point);
                    peak_index_A.removeIf(integer -> last_peekA_point == integer);
                }
                if (!mid_lind_a_origin_set.contains(peek_index)) {
                    peak_index_A.add(peek_index);
                    peekA_index_cache.add(peek_index);
                    if (peek_index > last_peekA_point) {
                        last_peekA_point = peek_index;
                    }
                }
            }

            peak_index_A = peak_index_A.stream().distinct().sorted().collect(Collectors.toList());
            if (tempRightIndex != -1) {
                last_right_indexA = tempRightIndex;
            }

            if (isShowLog) {
                System.out.println("filtered_mark aa的波峰索引:---" + peak_index_A + "-------" + aPointIndex);
            }

            // TODO: B 点
            int lastBigBmidSize = big_B_mid_index.size();
            for (int i = lastBmidIndex; i < aPointIndex; i++) {
//                if (filtered_markB[i] > filteredB_mid_value) {
                if (filtered_markB[i] > aveB.getMean()) {
                    big_B_mid_index.add(i);
                }
            }

            a = new int[0];//clean
            b = new int[0];
            if (big_B_mid_index.size() > 5 && lastBigBmidSize > 5) {
                if (big_B_mid_index.size() > lastBigBmidSize) {
                    a = big_B_mid_index.subList(lastBigBmidSize - 5, big_B_mid_index.size() - 1).stream().mapToInt(Integer::intValue).toArray();
                }
                if (big_B_mid_index.size() >= lastBigBmidSize + 1) {
                    b = big_B_mid_index.subList(lastBigBmidSize + 1 - 5, big_B_mid_index.size()).stream().mapToInt(Integer::intValue).toArray();
                }
            } else {
                if (big_B_mid_index.size() > lastBigBmidSize) {
                    a = big_B_mid_index.subList(lastBigBmidSize, big_B_mid_index.size() - 1).stream().mapToInt(Integer::intValue).toArray();
                }
                if (big_B_mid_index.size() >= lastBigBmidSize + 1) {
                    b = big_B_mid_index.subList(lastBigBmidSize + 1, big_B_mid_index.size()).stream().mapToInt(Integer::intValue).toArray();
                }
            }

            cha = new int[a.length];
            for (int i = 0; i < a.length; i++) {
                cha[i] = b[i] - a[i];
            }
            int tempRightIndexB = -1;
            interval_mid_line_index.clear();
            for (int i = 0; i < cha.length; i++) {
//                if (cha[i] > 150) {
                 if (cha[i] > 50) {
                    int leftIndex = a[i];
                    int rightIndex = b[i];
                    interval_mid_line_index.add(leftIndex);
                    interval_mid_line_index.add(rightIndex);
                    mid_lind_b_origin_set.add(leftIndex);
                    mid_lind_b_origin_set.add(rightIndex);
                    if (rightIndex > mid_lind_b_last_Index) {
                        mid_lind_b_last_Index = rightIndex;
                    }
                    mid_lind_b_index_cache.add(leftIndex);
                    mid_lind_b_index_cache.add(rightIndex);
                    tempRightIndexB = rightIndex;
                }
            }

            if (mid_lind_b_index_cache.size() > 0) {
                interval_mid_line_index.addAll(mid_lind_b_index_cache);
            }

            if (isShowLog) {
                interval_mid_line_index = interval_mid_line_index.stream().distinct().sorted().collect(Collectors.toList());
                System.out.println("DistanceAlgo:onMathRssiStepCount:interval_mid_line_index: b 交点" + interval_mid_line_index + "------apoint" + aPointIndex);
            }

            interval_mid_line_index.add(aPointIndex - 1);
            interval_mid_line_index = interval_mid_line_index.stream().distinct().sorted().collect(Collectors.toList());
            //TODO b 交点 找完。
            lastBmidIndex = aPointIndex;

            //TODO 找到 B 波峰，索引。
            List<Integer> peak_index_B = new ArrayList<>();
            if (last_peekB_point > 0 && peekB_index_cache.size() > 0) {
                peak_index_B.addAll(peekB_index_cache);
            }

//            1505, 1647 TODO 假设波峰波谷 计算正确
//            if (peekB_index_cache.contains(1505)){
//                peekB_index_cache.remove(1505);
//            }
//            if (peekB_index_cache.contains(1647)){
//                peekB_index_cache.remove(1647);
//            }

            for (int i = 1; i < interval_mid_line_index.size(); i++) {
                int left = interval_mid_line_index.get(i - 1);
                if (left < last_right_indexB) {
                    continue;
                }
                int right = interval_mid_line_index.get(i);
                //找到最大值，索引。
                int peek_index = left;
                double maxValue = filtered_markB[left];
                for (int j = left; j <= right; j++) {
                    if (filtered_markB[j] > maxValue) {
                        maxValue = filtered_markB[j];
                        peek_index = j;
                    }
                }
                if (maxValue > filtered_markB[last_peekB_point] && (peek_index - last_peekB_point < 100 ||
                        (mid_lind_b_last_Index > -1 && peek_index > mid_lind_b_last_Index && last_peekB_point > mid_lind_b_last_Index))) {
                    peekB_index_cache.remove(last_peekB_point);
                    peak_index_B.removeIf(integer -> last_peekB_point == integer);
                }
                if (!mid_lind_b_origin_set.contains(peek_index)) {
                    peak_index_B.add(peek_index);
                    peekB_index_cache.add(peek_index);
                    if (peek_index > last_peekB_point) {
                        last_peekB_point = peek_index;
                    }
                }
            }

            peak_index_B = peak_index_B.stream().distinct().sorted().collect(Collectors.toList());

            //todo 找到所有的 peak b
            if (tempRightIndexB != -1) {
                last_right_indexB = tempRightIndexB;
            }

            if (isShowLog) {
                System.out.println("filtered_markA的波峰索引:---" + peak_index_A);
                System.out.println("filtered_markB的波峰索引:---" + peak_index_B + "-------" + aPointIndex);
            }

            //计算 1圈的长度。。。。。。。。。当 A or B 单独 >= 2时。
            double oneCircle_indexlen_A = 0;
            double oneCircle_indexlen_B = 0;
            if (peak_index_A.size() > 3) {
                for (int i = 1; i < peak_index_A.size() - 2; i++) {
                    oneCircle_indexlen_A += peak_index_A.get(i + 1) - peak_index_A.get(i);
                }
                oneCircle_indexlen_A /= (peak_index_A.size() - 3);
            } else if (peak_index_A.size() > 1 && peak_index_A.size() <= 3) {
                for (int i = 0; i < peak_index_A.size() - 1; i++) {
                    oneCircle_indexlen_A += peak_index_A.get(i + 1) - peak_index_A.get(i);
                }
                oneCircle_indexlen_A /= (peak_index_A.size() - 1);
            }
            if (peak_index_B.size() > 3) {
                for (int i = 1; i < peak_index_B.size() - 2; i++) {
                    oneCircle_indexlen_B += peak_index_B.get(i + 1) - peak_index_B.get(i);
                }
                oneCircle_indexlen_B /= (peak_index_B.size() - 3);
            } else if (peak_index_B.size() > 1 && peak_index_B.size() <= 3) {
                for (int i = 0; i < peak_index_B.size() - 1; i++) {
                    oneCircle_indexlen_B += peak_index_B.get(i + 1) - peak_index_B.get(i);
                }
                oneCircle_indexlen_B /= (peak_index_B.size() - 1);
            }

            List<Integer> peaks_all = new ArrayList<>(peak_index_A);
            peaks_all.addAll(peak_index_B);
            peaks_all = peaks_all.stream().distinct().sorted().collect(Collectors.toList());


            //math  one_step_length
            one_step_length = 0;
            if (peaks_all.size() > 3) {
                int peakMathCount = 0;
                for (int i = 1; i < peaks_all.size() - 2; i++) {
//                        one_step_length += 30 / (step_count_origin_data.get( (int) Math.round(peaks_all.get(i + 1) / 10.0) - 1) - step_count_origin_data.get((int) Math.round(peaks_all.get(i) / 10.0)) - 1);
                    BigDecimal bg1 = new BigDecimal(peaks_all.get(i + 1) / 10.0).setScale(0, RoundingMode.HALF_EVEN);
                    BigDecimal bg2 = new BigDecimal(peaks_all.get(i) / 10.0).setScale(0, RoundingMode.HALF_EVEN);
                    double rightPeakStep = step_count_origin_data[Math.min(step_count_origin_data.length - 1, bg1.intValue() - 1)];
                    double leftPeakStep = step_count_origin_data[Math.min(step_count_origin_data.length - 1, bg2.intValue())];
                    if (rightPeakStep - leftPeakStep <=10){
                        continue;
                    }
                    peakMathCount++;
                    one_step_length += twoMarkPointDistance / (rightPeakStep - leftPeakStep - 1);
//                    System.out.println("DistanceAlgo:onMathRssiStepCount:"+one_step_length);
//                    System.out.println("DistanceAlgo:onMathRssiStepCount:2:"+one_step_length);
                    //TODO 2024-08-04 为什么 -1 ？？？？？
                }
//                one_step_length /= (peaks_all.size() - 3);
                one_step_length /= peakMathCount;
            } else if (peaks_all.size() > 1 && peaks_all.size() <= 3) {
                for (int i = 0; i < peaks_all.size() - 1; i++) {
                    BigDecimal bg1 = new BigDecimal(peaks_all.get(i + 1) / 10.0).setScale(0, RoundingMode.HALF_EVEN);
                    BigDecimal bg2 = new BigDecimal(peaks_all.get(i) / 10.0).setScale(0, RoundingMode.HALF_EVEN);

                    one_step_length += twoMarkPointDistance / (step_count_origin_data[bg1.intValue() - 1] - step_count_origin_data[bg2.intValue()] - 1);
                }
                one_step_length /= (peaks_all.size() - 1);
                for (int i = 0; i < peaks_all.size() - 1; i++) {
                    oneCircle_indexlen_A += peaks_all.get(i + 1) - peaks_all.get(i);
                }
                oneCircle_indexlen_A /= peaks_all.size();
                oneCircle_indexlen_A *= 2;
                // TODO: 8/6/24  根据经验！！！！！！！使用 A的值。。。。。
                oneCircle_indexlen_B = oneCircle_indexlen_A;
            } else {
                is_half_circle_long = 0;
            }

            if (isShowLog) {
                System.out.println("DistanceCalculator:mathDistance:one_step_length---::" + one_step_length + "---aPointIndex:" + aPointIndex);
            }

            // TODO 计算距离：：：：
            //distance = len/onecircleLen * oneCirclelen
            if (is_half_circle_long == 0) {
                distance_origin[aPointIndex] = 0;
                is_half_circle_long = 1;
            } else {
                double cur_distance;
//                if (peak_index_B.get(0) <= peak_index_A.get(0)) {
                if (peak_index_A.size() > 0 && peak_index_B.size() > 0 && peak_index_B.get(0) <= peak_index_A.get(0)) {
//                if (peak_index_B.get(0) <= peak_index_A.get(0) && peak_index_A.size() > 1) {
                    if (Math.floor(peak_index_A.get(peak_index_A.size() - 1) / 10.0) <= stepNextReceiveIndex) {
                        int end = (int) Math.floor(aPointIndex / 10.0) - 1;
//                        if (end < step_count_origin_data.size()) {
                        if (end < stepNextReceiveIndex) {
                            cur_distance = (peak_index_A.get(0) / oneCircle_indexlen_A) * twoMarkPointDistance * 2
                                    + (peak_index_A.size() - 1) * twoMarkPointDistance * 2
//                                    + one_step_length * (step_count_origin_data[(int) Math.floor(aPointIndex / 10.0) - 1] - step_count_origin_data[(int) Math.floor(peak_index_A.get(peak_index_A.size() - 1) / 10.0) - 1]);
                                    + one_step_length * (step_count_origin_data[(int) Math.floor(aPointIndex / 10.0) - 1] - step_count_origin_data[Math.max(0, (int) Math.floor(peak_index_A.get(peak_index_A.size() - 1) / 10.0) - 1)]);
                            distance_origin[aPointIndex] = cur_distance;
                        } else {
                            cur_distance = (peak_index_A.get(0) / oneCircle_indexlen_A) * twoMarkPointDistance * 2
                                    + (peak_index_A.size() - 1) * twoMarkPointDistance * 2
                                    + one_step_length * (step_count_origin_data[stepNextReceiveIndex - 1] - step_count_origin_data[Math.max(0, (int) Math.floor(peak_index_A.get(peak_index_A.size() - 1) / 10.0) - 1)]);
//                                    + one_step_length * (step_count_origin_data[stepNextReceiveIndex - 1] - step_count_origin_data[(int) Math.floor(peak_index_A.get(peak_index_A.size() - 1) / 10.0)]);
                            distance_origin[aPointIndex] = cur_distance;
                        }
                    } else {
                        cur_distance = (peak_index_A.get(0) / oneCircle_indexlen_A) * twoMarkPointDistance * 2
                                + (peak_index_A.size() - 1) * twoMarkPointDistance * 2
                                + ((aPointIndex - 1 - peak_index_A.get(peak_index_A.size() - 1)) / oneCircle_indexlen_A) * twoMarkPointDistance * 2;
                        distance_origin[aPointIndex] = cur_distance;
                    }
//                } else {
                } else if (peak_index_B.size() > 0){
                    if (Math.floor(peak_index_B.get(peak_index_B.size() - 1) / 10.0) <= stepNextReceiveIndex) {
                        int end = (int) Math.floor(aPointIndex / 10.0) - 1;
                        if (end < stepNextReceiveIndex) {
                            cur_distance = (peak_index_B.get(0) / oneCircle_indexlen_B) * twoMarkPointDistance * 2
                                    + (peak_index_B.size() - 1) * twoMarkPointDistance * 2
//                                    + one_step_length * (step_count_origin_data[(int) Math.floor(aPointIndex / 10.0) - 1] - step_count_origin_data[(int) Math.floor(peak_index_B.get(peak_index_B.size() - 1) / 10.0) - 1]);
                                    + one_step_length * (step_count_origin_data[(int) Math.floor(aPointIndex / 10.0) - 1] - step_count_origin_data[Math.max(0, (int) Math.floor(peak_index_B.get(peak_index_B.size() - 1) / 10.0) - 1)]);
                            distance_origin[aPointIndex] = cur_distance;
                        } else {
                            cur_distance = (peak_index_B.get(0) / oneCircle_indexlen_B) * twoMarkPointDistance * 2
                                    + (peak_index_B.size() - 1) * twoMarkPointDistance * 2
//                                    + one_step_length * (step_count_origin_data[stepNextReceiveIndex - 1] - step_count_origin_data[(int) Math.floor(peak_index_B.get(peak_index_B.size() - 1) / 10.0) - 1]);
//                                    + one_step_length * (step_count_origin_data[stepNextReceiveIndex - 1] - step_count_origin_data[(int) Math.floor(peak_index_B.get(peak_index_B.size() - 1) / 10.0)]);
                                    + one_step_length * (step_count_origin_data[stepNextReceiveIndex - 1] - step_count_origin_data[Math.max(0, (int) Math.floor(peak_index_B.get(peak_index_B.size() - 1) / 10.0) - 1)]);
                            distance_origin[aPointIndex] = cur_distance;
                        }
                    } else {
                        cur_distance = (peak_index_B.get(0) / oneCircle_indexlen_B) * twoMarkPointDistance * 2
                                + (peak_index_B.size() - 1) * twoMarkPointDistance * 2
                                + ((aPointIndex - 1 - peak_index_B.get(peak_index_B.size() - 1)) / oneCircle_indexlen_B) * twoMarkPointDistance * 2;
                        distance_origin[aPointIndex] = cur_distance;
                    }
                }
                distance_origin_fix[aPointIndex] = distance_origin[aPointIndex];
            }


            //TODO 距离修正。。。。当前 point - lastpeek  distance 应该 < twoMarkDistance 否则，上次拐点计算的有问题。
            int finalRssi_point_index = aPointIndex;
            int zuijin_guaidian_index = peaks_all.stream().filter(val -> val < finalRssi_point_index).mapToInt(Integer::intValue).max().orElse(-1);
            if (zuijin_guaidian_index < 0) {
                zuijin_guaidian_index = 0;
            }
            double shangci_guaidian = distance_origin[zuijin_guaidian_index];
            if (Math.abs(shangci_guaidian - distance_origin[aPointIndex]) > twoMarkPointDistance) {
//                System.err.println("DistanceAlgo:onMathRssiStepCount:????? 不应该超过 上拐点"+zuijin_guaidian_index+"...nowPoint:"+aPointIndex+"修正前的距离："+distance_origin_fix[aPointIndex]
//                        +"修正后："+(shangci_guaidian + twoMarkPointDistance)+"修正的值："+(distance_origin_fix[aPointIndex]-shangci_guaidian - twoMarkPointDistance));
                distance_origin_fix[aPointIndex] = (shangci_guaidian + twoMarkPointDistance);
            }


            //TODO 距离修正 2 ????? 之前的距离 > 当前的距离。。。为什么不考虑 3550之后的呢？？？ 之前的统计可能有问题计算过大也是有可能。 3660大概是终点，是最大值，靠近。波峰。
            // 这个有用吗？？？？
            // 计算 速度的作用。。。。。为什么修正？？？之后的不 fix ?......
            if (aPointIndex > 2 && aPointIndex < 3550) {
                double distance_last = distance_origin_fix[aPointIndex - 1];
                double distance_cur = distance_origin_fix[aPointIndex];
                if (distance_last > distance_cur) {
//                    System.out.println("DistanceAlgo:onMathRssiStepCount:"+ aPointIndex+"---"+distance_last+"---cur:"+distance_cur);
                    distance_origin_fix[aPointIndex] = distance_last;
                }
            }

            //统计最大值，，，仅仅展示用
            if (distance_max < distance_origin_fix[aPointIndex]) {
                distance_max = distance_origin_fix[aPointIndex];
                distance_max_index = aPointIndex;
            }
            if (isShowLog) {
                System.out.println("DistanceAlgo:onMathRssiStepCount:点--距离：" + aPointIndex + "::" + distance_origin_fix[aPointIndex]);
            }

            //计算 speed
            if (aPointIndex % 10 == 0) {
                distance_origin_for_second.add(distance_origin_fix[aPointIndex]);
                Double lastDis = distance_origin_for_second.get(distance_origin_for_second.size() - 1);
                if (lastDis == null) {
                    lastDis = 0.0;
                }
                if (lastDis < 5) {
                    speed_for_second.add(0.0);
                    if (isShowLog) {
                        System.out.println("speed-----21   >" + 0 + "   apoint:" + aPointIndex);
                    }
                } else {
                    int last_5_second = (int) (aPointIndex / 10.0) - 5;
                    double dis_5_second = lastDis - distance_origin_for_second.get(last_5_second);
                    double cur_speed = dis_5_second / 5.0;
                    if (cur_speed - last_speed > 0.5) {
                        cur_speed = last_speed + 0.1;
                    }
                    if (cur_speed < 0) {
                        cur_speed = 0;
                    }
                    last_speed = cur_speed;
                    speed_for_second.add(cur_speed);
                    if (isShowLog) {
                        System.out.println("speed-----22  >" + cur_speed + "   apoint:" + aPointIndex);
                    }
                }
            }
//                System.out.println("DistanceCalculator:mathDistance:oneCircle_indexlen_A:"+oneCircle_indexlen_A+",,,oneCircle_indexlen_B:"+oneCircle_indexlen_B);
        }

//        if (isShowLog) {
//            System.out.println("speed:----->"+last_speed+"      aPointIndex:"+aPointIndex);
//        }
    }

    public static class Result {
        double[] distance_fix;
        double distanceRes;
        double distanceResIndex;
        double distanceMax;
        double distanceMaxIndex;
        double stepRes;
        double oneStepLen;
        List<Double> speed;


        public Result() {
        }

        public Result(double[] distance_fix, double distanceRes, double distanceResIndex, double distanceMax, double distanceMaxIndex, double stepRes, double oneStepLen, List<Double> speed) {
            this.distance_fix = distance_fix;
            this.distanceRes = distanceRes;
            this.distanceResIndex = distanceResIndex;
            this.distanceMax = distanceMax;
            this.distanceMaxIndex = distanceMaxIndex;
            this.stepRes = stepRes;
            this.oneStepLen = oneStepLen;
            this.speed = speed;
        }

        @Override
        public String toString() {
            return "Result{" +
//                    "distance_fix=" + Arrays.toString(distance_fix) +
                    ", distanceRes=" + distanceRes +
                    ", distanceResIndex=" + distanceResIndex +
                    ", distanceMax=" + distanceMax +
                    ", distanceMaxIndex=" + distanceMaxIndex +
                    ", stepRes=" + stepRes +
                    ", oneStepLen=" + oneStepLen +
                    ", speed=" + speed +
                    '}';
        }
    }

}
